#include "BoidAirCraft.h"

//! constructor.
BoidAirCraft::BoidAirCraft(
	OgreOde::Body *mBody, float thrust, float lift,
	float minDrag, float maxDrag, float gravity,
	const Ogre::Radian& maxPitch, const Ogre::Radian& maxRoll) :
BoidBase(mBody), mVelocity(0, 0, 0), mThrust(thrust), mLift(lift),
mMinDragOffset(minDrag - maxDrag), mMaxDrag(maxDrag), mGravity(gravity),
mMaxPitch(maxPitch), mMaxRoll(maxRoll)
{
}


//! steer boid base on desired steering vector.
void BoidAirCraft::steerBoid(const Ogre::Vector3& desiredVector)
{
	// this implements a very basic aircraft simulation.
	Ogre::Vector3 normDesiredVector(desiredVector);
	float lenght = normDesiredVector.normalise();

	// first we find out if we need to change direction.
	if (lenght > 1e-08f)
	{
		float forwardness = getForwardVector().dotProduct(normDesiredVector);

		if (forwardness < 999e-03f)
		{
			// our plane is not flying towards the desired direction.
			// let's try to fix it.

			// first we try to find if we have to roll.
			// since the upness will give a value as -1 to 1,
			// we can cheat with this trick to use it as our pitch magnitude.
			float upness = getUpVector().dotProduct(normDesiredVector);
			Ogre::Quaternion q1 = mBody->getOrientation();
			Ogre::Quaternion q2(-mMaxPitch * upness,Ogre::Vector3::UNIT_X);
			mBody->setOrientation(q1*q2);
			//mBody->pitch(-mMaxPitch * upness);

			// then we do the rolling with the same trick.
			float leftness = getRightVector().dotProduct(normDesiredVector);
			q1 = mBody->getOrientation();
			Ogre::Quaternion q3(-mMaxRoll * leftness,Ogre::Vector3::UNIT_Z);
			mBody->setOrientation(q1*q3);
			//mBody->roll(-mMaxRoll * leftness);
		}
		else if (getUpVector().dotProduct(Ogre::Vector3::UNIT_Y) < 999e-03f)
		{
			float leftness = getRightVector().dotProduct(Ogre::Vector3::UNIT_Y);
			Ogre::Quaternion q1 = mBody->getOrientation();
			Ogre::Quaternion q2(-mMaxRoll * leftness,Ogre::Vector3::UNIT_Z);
			mBody->setOrientation(q1*q2);
			//mBody->roll(-mMaxRoll * leftness);
		}
	}
	else if (getUpVector().dotProduct(Ogre::Vector3::UNIT_Y) < 999e-03f)
	{
		float leftness = getRightVector().dotProduct(Ogre::Vector3::UNIT_Y);
		Ogre::Quaternion q1 = mBody->getOrientation();
		Ogre::Quaternion q2(-mMaxRoll * leftness,Ogre::Vector3::UNIT_Z);
		mBody->setOrientation(q1*q2);
		//mBody->roll(-mMaxRoll * leftness);
	}

	// now we apply the flight simulation.
	Ogre::Vector3 normVelocity(mVelocity);
	float maxLift = mLift * normVelocity.normalise();

	Ogre::Vector3 forward(getForwardVector());
	Ogre::Vector3 up(getUpVector());
	Ogre::Vector3 gravity(0.0f, mGravity, 0.0f);
	float gravityUp = -(up.dotProduct(gravity));
	if (gravityUp > maxLift) gravityUp = maxLift;
	else if (gravityUp < -maxLift) gravityUp = -maxLift;

	float thrustMag = mThrust - forward.dotProduct(mVelocity);
	if (thrustMag < 0.0f) thrustMag = 0.0f;
	Ogre::Vector3 thrust = forward * thrustMag;
	Ogre::Vector3 lift = up * ((gravityUp < maxLift) ? gravityUp : maxLift);
	Ogre::Vector3 drag = mVelocity * -(mMaxDrag + mMinDragOffset * forward.dotProduct(normVelocity));
	
	mVelocity += thrust;
	mVelocity += lift;
	mVelocity += drag;
	mVelocity += gravity;
	//mBody->setPosition(mBody->getPosition() * mVelocity);
	mBody->setLinearVelocity(mBody->getOrientation() * mVelocity * 10);
}
